#pragma once
#include "../../roboter/include/dynamica/core.hpp"
#include "../../algorithm/include/SolveHFVC.hpp"
#include "../../controller/include/ForceControl.hpp"
#include "../../driver/include/force_control_hardware.h"
using namespace dynamica;
typedef struct Dimension
{
    int Actualized;
    int UnActualized;
    int SlidingFriction;
    int Lambda;
    int Generalized;

    void init()
    {
        Actualized = 0;
        UnActualized = 0;
        SlidingFriction = 0;
        Lambda = 0;
        Generalized = 0;
    }
}DimensionTypedef;

typedef struct RbHand
{
    Vec<3> vel_cmd_world;
    Vec<3> force_cmd_world;
    Vec<6> force_set;
    Vec<6> force_fb;
    Vec<7> pose_fb;
    Vec<7> pose_set;

    void init()
    {
        vel_cmd_world.setZero();
        force_cmd_world.setZero();
        force_set.setZero();
        force_fb.setZero();
        pose_fb.setZero();
        pose_set.setZero();
    }
}RbHandDataTypedef;

class BlockTilting
{
private:
    HFVCDataTypedef _action;
    RbHandDataTypedef _hand;
public:
/*  record  */
    Clock::time_point _time0;
    std::ofstream _file;
    bool _print_flag = false;
    std::string fullpath;
/*  controller  */
    ForceControlController *_controller;

/*  A. variabels  */
    Vec<3>  p_obj_world;//position of the obj in the world
    Vec<3>  p_obj0_world;//initial position of the obj in the world
    Quaterniond  qtn_obj_world;//quaternion of the obj in the world
    Vec<3>  p_h_world;//position of the hand tip in the world
    Vec<3>  p_h0_world;//initial position of the hand tip in the world

    Vec<6>  tw_obj_obj;//obj body twist in the obj frame;
    Vec<3>  v_h_world;//hand linear vel in the world;
    Vec<10> q_sys_state;
    Vec<9>  v_sys_gene_vel;
    Vec<9>  f_sys_gene_force;
    Mat<10,9> Omega_map_v2qd;
    Mat<4,3>  E_qo;
    Mat<3,3>  sel_matrix;
    Mat<3,3>  R_obj_world;

/*  B. goal description  */
    Vec<3>  p_tc_world;//any point on the line of contact
    Vec<3>  w_axis_world;//axis of rotation
    Vec<6>  tw_obj_goal_world;//goal of obj motion
    Mat<6,6> adjoint_goal_inv;//adjoint transformation associated with W-O-g-1
    Mat<6,9> G_goal;//goal description
    Vec<6> bG_goal;//goal description
    Vec<3> tilt_direction;//goal rotation direction base on world frame
    Vec<3> rotate_axis;//axis of block rotation
    Vec<3> v_obj_world;//v + w = twist
    Vec<3> w_obj_world;//v + w = twist
    double goal_vel;//block rotation velocity  uint: rad/s
/*  C. natural constrains  */
/* 1-holonomic constraints */
    Vec<3>  p_hc_world;
    Vec<3>  p_hc_obj;
    Vec<3>  p_tc1_world;
    Vec<3>  p_tc2_world;
    Vec<3>  p_tc1_obj;
    Vec<3>  p_tc2_obj;
    Mat<9,10> jac_phi_q;

/* 2-Newton's 2nd law */
    Vec<3>  lambda_hc_world;
    Vec<3>  lambda_tc1_world;
    Vec<3>  lambda_tc2_world;
    Vec<9>  f_reaction; //associated with the holonomic constraints 
    Vec<9>  F_world;// contains gravity of obj Go and robot hand Gh
    Vec<3>  F_Go;
    Vec<3>  F_Gh;
    double   hand_mass;
    double   block_mass;
    double   _gravity;  

/*  D. guard conditions */
    Eigen::MatrixXd _A;
    Eigen::MatrixXd _Aeq;
    Eigen::VectorXd _b_A;
    Eigen::VectorXd _b_Aeq;
    Vec<3> Z_unit;//Z = [0 0 1]T 
    Vec<3> friction_dir[6];//friction direction
    double u_hc,u_tc;//estimated minimal possible friction coefficient
    double n_min;//normal force minimum threshold
    int fric_coneside; //default value is 6.
    DimensionTypedef _dim;
/*  other info */
    int num_seeds;
    int print_level;
    double block_edge;
    bool run_first_task;  
    bool is_end_task;  // determine if the end task is gonna run.
    bool is_stop_moving; // determine if the robot is gonna stop.
/*  function    */
    BlockTilting();
    ~BlockTilting();    
    void init(ForceControlController *_ctrl, std::chrono::high_resolution_clock::time_point  _time);
    void cal_hand_cmd(HFVCDataTypedef *_hfvc, RbHandDataTypedef *_hand);
    Mat<9,10> cal_jac_phi_q(Vec<3> &p_wo, Vec<4> &q_wo, Vec<3> &p_wh, Vec<3> &p_ohc, Vec<3> &p_wtc_1, Vec<3> &p_wtc_2, Vec<3> &p_otc1, Vec<3> &p_otc2);
    void init_task(bool is_ext_param = true);
    void run_task();
    void end_task();
};


